#include <arch/psci.h>
#include <core/smp.h>
#include <core/dprintf.h>

void smp_boot_secondary_cpu(void)
{
#ifdef CONFIG_ENABLE_SMP
    extern uint64_t _start_secondary;
    uint64_t        i;
    uint32_t        initialcores = smp_current_cores();
    uint64_t entry = allocator_pbase((uint64_t)&_start_secondary);

    int32_t err = psci_init();
    if (err) {
        return;
    }

    for (i = 1; i < CONFIG_SMP_CORES; i++) {
        word_t mpidr = scr_read_MPIDR();
        mpidr        = (mpidr & ~0xFFU) | (i & 0xFFU);

        dprintf("mpidr: %x, entry: %x\n", mpidr, entry);
	int32_t error = psci_cpu_on(mpidr, entry);
        if (error) {
            dprintf("enable Core#%d failed,ret code is %x \n", i, err);
            return;
        } else {
            dprintf("enable Core#%d \n", i);
        }

        while (smp_current_cores() != (initialcores + i)) {
        }
	dprintf("Core#%d is ready!\n", i);
    }
#endif
}
